#include <msp430.h>				

#include "car.h"
#include "uart.h"
#include "timer.h"
#include "UltraWave.h"

void SysInit(void);
int pos = 0, degree = 0;

#define VAL_LIMIT(val, min, max) \
    if (val <= min)       \
    {              \
        val = min;        \
    }              \
    else if (val >= max)     \
    {              \
        val = max;        \
    }

/**
 * @brief pid 控制结构体
 * @note
 * 1. 使用标准公式u(k) = (kp)e(k) + (ki)T/∑e(k) + (kd)/T(e(k) - e(k-1))
 * 2. 如果不需要某个参数，则将控制参数设置为0
 * 3. 输入、输出和测量值不在该结构体中，测量值自行进行惯性滤波
 * 4. 大部分参数需要初始化
 */
typedef struct
{
    float kp;
    float out_limit;
    float p_limit;
    float ki;
    float i_limit;
    float i_history;
    float kd;
    float d_limit;
    float d_alpha;
    float d_out_last;
    float ts;
    float err_last;
} llz_pid_t;

/**
 * @brief PID参数初始化
 *
 * @param[in] pid_params PID参数
 */
void pid_init(llz_pid_t *params)
{
    params->kp = 1;
    params->ki = 0;
    params->kd = 0;
    params->p_limit = 50;
    params->i_limit = 0;
    params->d_limit = 0;
    params->out_limit = 50;
    params->ts = 50;
    params->d_alpha = 0;
    params->i_history = 0;
    params->err_last = 0;
    params->d_out_last = 0;
}

/**
 * @brief 带有输出限制的不完全微分PID控制算法
 *
 * @param[in,out] pid_params PID控制参数
 * @param[in] set 设定值/输入
 * @param[in] filtered_measure 惯性滤波后的测定值/实际
 * @return float 输出
 */
float pid_algorithm(llz_pid_t *pid_params, float set, float filtered_measure)
{
    float *kp = &pid_params->kp;
    float *ki = &pid_params->ki;
    float *kd = &pid_params->kd;
    float *ts = &pid_params->ts;
    float *fi = &pid_params->i_history;
    float *err_old = &pid_params->err_last;
    float *fd_alpha = &pid_params->d_alpha;
    float *fd_old = &pid_params->d_out_last;
    float fp, fd, out, err;

    err = set - filtered_measure;

    fp = (*kp) * err;  ///< P
    VAL_LIMIT(fp, -pid_params->p_limit, pid_params->p_limit);

    *fi += (*ts) * (*ki) * err; ///< I
    VAL_LIMIT((*fi), -pid_params->i_limit, pid_params->i_limit);

    fd = (*kd) * (err - *err_old) / (*ts); ///< D
    fd = fd * (1 - (*fd_alpha)) + (*fd_old) * (*fd_alpha); ///< filtered_D
    VAL_LIMIT(fd, -pid_params->d_limit, pid_params->d_limit);

    *fd_old = fd;
    *err_old = err;
    out = fp + (*fi) + fd;
    VAL_LIMIT(out, -pid_params->out_limit, pid_params->out_limit);

    return out;
}

llz_pid_t g_pid;
llz_pid_t g_pid_2;

/**
 * blink.c
 */

float g_rotate;
float g_div;
int g_all;
int basic_speed = 30;
int final_speed_l;
int final_speed_r;
void main(void)
 {
	volatile unsigned int i;		// volatile to prevent optimization


	SysInit();
	pid_init(&g_pid);
	pid_init(&g_pid_2);
	UART_Init(); // 初始化串口1，用于调试,外接蓝牙
	uart0_init(); // 初始化串口0， 用于外接open mv
	CarInit();
	TimerA1Init(); // 用于时间测量
	UltraWaveInit();
	UART_Puts("this is uart1, system start...\r\n");
	P1DIR |= BIT0;
	P4DIR |= BIT7;
	P1OUT &= ~BIT0;
	P4OUT &= ~BIT7;

	u32 ms = millis();
	CarStart(BACK, 30);
      P1OUT |= BIT0;
      P4OUT |= BIT7;
    u32 ms_start = millis();
	while(1)
	{
	    if (millis() - ms_start > 11650)
	    {
	        CarStart(BACK, 0);
	    }
	    else if(millis() - ms > 20)
	    {
	        ms = millis();
	        P1OUT ^= BIT0;
	        g_rotate = pid_algorithm(&g_pid, 50, pos);
	        g_div = pid_algorithm(&g_pid_2, 50, degree);
	        g_all = (int)(g_rotate + g_div / 2);
	        final_speed_l = basic_speed - g_all;
	        final_speed_r = basic_speed + g_all;
	        VAL_LIMIT(final_speed_l, 10, 80)
	        VAL_LIMIT(final_speed_r, 10, 80)
	        LeftMotorSetSpeed(final_speed_r); // 数字对应速度的关系？
	        RightMotorSetSpeed(final_speed_l); // 数字对应速度的关系？
	    }
	}
}

//---------------------------------------------

void SysInit(void)
{
  StopWdt();//关闭看门狗
  increaseVCoreToLevel2();//内核电压升高
  initClockTo16MHz();//将内核主频设定到16M
  __enable_interrupt();    //开启总中断
}
